{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0",
   "metadata": {},
   "source": [
    "# The mobile base"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1",
   "metadata": {},
   "source": [
    "Reachy 2 is mounted on a mobile base!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8361d4e8",
   "metadata": {},
   "source": [
    "> Make sure you have **enough clear space around the robot** before starting the tutorial. Required space is about 1.5m around the robot in each direction."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2",
   "metadata": {},
   "source": [
    "## Initialize your robot\n",
    "\n",
    "First connect to your robot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3",
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk import ReachySDK\n",
    "import time\n",
    "\n",
    "reachy = ReachySDK(host='localhost')  # Replace with the actual IP"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4",
   "metadata": {},
   "source": [
    "Let's check what contains the mobile base part:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ba742f7c",
   "metadata": {},
   "source": [
    "## Odometry"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6",
   "metadata": {},
   "source": [
    "## Move around with gotos"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7",
   "metadata": {},
   "source": [
    "The goto commands and goto-based commands described below follow all the rules you saw in the [goto introduction tutorial](2_goto_introduction.ipynb)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8",
   "metadata": {},
   "source": [
    "### Goto and odometry\n",
    "\n",
    "The goto function is used to place the mobile_base at a relative position and orientation to its odometry, set when the robot is switched on. To be sure, you can reset the odometry before calling the function. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c9a89028",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.reset_odometry()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f50a38ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.odometry"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b2c222c1",
   "metadata": {},
   "source": [
    "Let's turn on the mobile to be able to move is around"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98084948",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.turn_on()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e960c060",
   "metadata": {},
   "source": [
    "The robot is currently positionned at x=0, y=0, theta=0.  \n",
    "If you want to move forward again the robot, you need to increase the x value (value is in meters):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Move 20 cm forward\n",
    "a = reachy.mobile_base.goto(x=0.2, y=0.0, theta=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "74d3ff75",
   "metadata": {},
   "source": [
    "Now, request `goto(0, 0, 0)`. The robot will return to its previous position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f9995821",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.goto(x=0.0, y=0.0, theta=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f222b61",
   "metadata": {},
   "source": [
    "All the positions are relative to the fixed odometry coordinate system of the mobile base, set at the start of the robot of after a `reset_odometry()` asked by the user.\n",
    "\n",
    "So if you do:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4098156",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Move 30cm forward, to reach x=30cm\n",
    "reachy.mobile_base.goto(x=0.3, y=0.0, theta=0.0)\n",
    "\n",
    "# Go back by 10cm, to reach x=20cm\n",
    "reachy.mobile_base.goto(x=0.2, y=0.0, theta=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "03070c8b",
   "metadata": {},
   "source": [
    "The mobile is first going to the position x=30cm in the odometry frame. We then ask for it to get to the position x=20cm in this same frame, so the mobile base is going backward by 10cm to reach its new target.  \n",
    "\n",
    "Let's do the same by resetting the odometry between the two commands:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a69ec790",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Move 30cm forward, to reach x=30cm in the current odometry frame\n",
    "reachy.mobile_base.goto(x=0.3, y=0.0, theta=0.0, wait=True)\n",
    "time.sleep(0.5)\n",
    "print(f\"x position before odometry reset: {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "\n",
    "# Reset odometry\n",
    "reachy.mobile_base.reset_odometry()\n",
    "time.sleep(0.5)\n",
    "print(f\"x position after odometry reset: {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "\n",
    "# Move 20cm forward, to reach x=20cm in the new current odometry frame\n",
    "reachy.mobile_base.goto(x=0.2, y=0.0, theta=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "924faa77",
   "metadata": {},
   "source": [
    "As we reset the odometry between the two commands, the mobile base odometry position is reset to x=0cm before the second command. It will then reach x=20cm in the new frame, so move forward by 20cm.\n",
    "\n",
    "We recommend experimenting with this concept to get familiar."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0c049342",
   "metadata": {},
   "source": [
    "### Relative moves"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c504dda6",
   "metadata": {},
   "source": [
    "You can also decide to assign movements to the robot based on its current position and not on its odometry. \n",
    "\n",
    "Two methods are available to give relative orders:\n",
    "- **`translate_by()`**: to give translations orders. \n",
    "- **`rotate_by()`**: to give rotations orders  \n",
    "\n",
    "These methods work like all gotos, and return a GoToId.  \n",
    "\n",
    "The translation or rotation is computed based on the current position if no goto is playing, or on the position required for the last queued or playing goto in case gotos are not finished.\n",
    "\n",
    "Let's try some examples to better understand how it works."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a109318",
   "metadata": {},
   "source": [
    "#### translate_by()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b0f81ca3",
   "metadata": {},
   "source": [
    "Send the mobile base back the odometry frame origin first, and reset it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "096afcae",
   "metadata": {},
   "outputs": [],
   "source": [
    "a = reachy.mobile_base.goto(x=0, y=0.0, theta=0.0, wait=True, timeout=10)\n",
    "time.sleep(0.5)\n",
    "reachy.mobile_base.reset_odometry()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "57296096",
   "metadata": {},
   "source": [
    "Now, we are going to compare a `goto()` to a `translate_by()` command.  \n",
    "\n",
    "If we check the odometry, we will see the mobile_base at the origin of the current frame (that we have just reset):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "44ff2028",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "49217426",
   "metadata": {},
   "source": [
    "You can move forward to the position x=20cm by asking a translation of 20cm on the x-axis:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d3c39898",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.translate_by(x = 0.2, y = 0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a8576bf",
   "metadata": {},
   "source": [
    "Because you started from the origin, the result is the same as asking a `goto(x=0.2, y=0, theta=0)`.\n",
    "If we send this goto:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95b6785c",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.goto(x=0.2, y=0.0, theta=0.0, wait=True, timeout=10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f7357191",
   "metadata": {},
   "source": [
    "The mobile does not move, because we are already at this position. We can check this using the odometry:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d8e9218",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "280c8621",
   "metadata": {},
   "source": [
    "But if we ask a new translation of 20cm, the mobile base will go forward:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec4b7d8a",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.translate_by(x = 0.2, y = 0.0, wait=True, timeout=5)\n",
    "time.sleep(0.5)\n",
    "\n",
    "# Read odometry\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8c4a8d8b",
   "metadata": {},
   "source": [
    "#### rotate_by()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "383b2cd3",
   "metadata": {},
   "source": [
    "The `rotate_by()` method works quite the same way as the `translate_by()`, unlike it is for rotations.  \n",
    "For example, you can go back to the initial position then rotate the mobile base. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f04c56e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Go back to the initial position\n",
    "reachy.mobile_base.goto(x=0.0, y=0.0, theta=0.0, wait=True)\n",
    "\n",
    "# Rotation to be at 90 degrees in the frame\n",
    "reachy.mobile_base.goto(x=0.0, y=0.0, theta=90.0, wait=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a798a727",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c56e122",
   "metadata": {},
   "source": [
    "Now, the mobile base can be rotated 90° from its current position, allowing to get a odometry with a theta = 0°. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "84e18a9f",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.rotate_by(theta=-90.0, wait=True)\n",
    "time.sleep(0.5)\n",
    "\n",
    "# Check the odometry\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3a5aa250",
   "metadata": {},
   "source": [
    "#### Choose the right method!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1d4f8b2c",
   "metadata": {},
   "source": [
    "Be careful with the method you use. For example, those two sequences have completely different results:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "980b6b34",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.goto(x=0, y=0.0, theta=0.0, timeout=10)\n",
    "reachy.mobile_base.goto(x=0.2, y=0.0, theta=0.0, timeout=10)\n",
    "reachy.mobile_base.goto(x=0, y=0.4, theta=0.0, timeout=10)\n",
    "reachy.mobile_base.goto(x=0, y=0, theta=90.0, timeout=10, wait=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8dd41c6f",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63ac618e",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.goto(x=0, y=0.0, theta=0.0, timeout=10)\n",
    "reachy.mobile_base.translate_by(x=0.2, y=0.0, timeout=10)\n",
    "reachy.mobile_base.translate_by(x=0, y=0.4, timeout=10)\n",
    "a=reachy.mobile_base.rotate_by(theta=90.0, timeout=10, wait=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c439d92",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 2)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cc446efe",
   "metadata": {},
   "source": [
    "### Goto tolerances"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bdb6d85b",
   "metadata": {},
   "source": [
    "Unlike the arms and head whose movements duration is based on a duration argument, a mobile base goto duration is based on the tolerances and timeout arguments you can choose.  \n",
    "\n",
    "You can modify two different tolerances:\n",
    "- the **distance_tolerance**: defines the maximum distance to the (x, y) position in meters to consider the goto as finished (even if movement to target is not over yet)\n",
    "- the **angle_tolerance**: defines the angle distance to the theta rotation to consider the goto as finished (even if movement to target is not over yet) *- units based on the degrees argument, in degrees by default*"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f19e1c7d",
   "metadata": {},
   "source": [
    "> Default distance_tolerance is **0.05 meter**  \n",
    "> Default angle_tolerance is **5 degrees**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ccf05159",
   "metadata": {},
   "source": [
    "Be within those tolerances will raise the flag that the goto is considered as finished, which **does not mean the movement is over**! You can start a new goto without finishing this one, but if no other goto has been sent, the mobile base will try to get as close as possible to the target.  \n",
    "As it is easier to understand with examples, let's take some:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6a280c26",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Going back to the base position\n",
    "reachy.mobile_base.goto(x=0, y=0.0, theta=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "98c609fc",
   "metadata": {},
   "source": [
    "We are first going to send a target to x=0.6m, with a tolerance of 20cm. \n",
    "Put the `wait` argument to True to wait for the *goto* to be finished:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66989d2e",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.goto(x=0.6, y=0.0, theta=0.0, distance_tolerance=0.2, wait=True)\n",
    "\n",
    "odom_goto_end = reachy.mobile_base.odometry\n",
    "print(\"Odometry after the goto is declared as finished:\")\n",
    "print(f\"'x': {round(odom_goto_end['x'], 3)}\")\n",
    "print(f\"'y': {round(odom_goto_end['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom_goto_end['theta'], 3)}\")\n",
    "\n",
    "time.sleep(1)\n",
    "odom_move_end = reachy.mobile_base.odometry\n",
    "print(\"Odometry after the movement finished:\")\n",
    "print(f\"'x': {round(odom_move_end['x'], 3)}\")\n",
    "print(f\"'y': {round(odom_move_end['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom_move_end['theta'], 3)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3fd663bc",
   "metadata": {},
   "source": [
    "When the goto was **declared as finished**, to mobile base was **still receiving commands** to try to get as close to the target position as it could, so the **movement was not finished**. Simply, you can see the odometry values are within the tolerances, so the robot is ready to receive a new goto goal.  \n",
    "\n",
    "This can be used when chaining gotos.  \n",
    "\n",
    "For example, if we do this chain of gotos with the default tolerances:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3442fe70",
   "metadata": {},
   "outputs": [],
   "source": [
    "goto1 = reachy.mobile_base.goto(x=0.0, y=0.0, theta=0.0)\n",
    "goto2 = reachy.mobile_base.goto(x=0.6, y=0.0, theta=0.0)\n",
    "goto3 = reachy.mobile_base.goto(x=-0.2, y=0.0, theta=0.0)\n",
    "goto4 = reachy.mobile_base.goto(x=0.3, y=0.0, theta=0.0)\n",
    "\n",
    "while not reachy.is_goto_finished(goto1):\n",
    "    time.sleep(0.05)\n",
    "odom1 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto1:\")\n",
    "print(f\"'x': {round(odom1['x'], 3)}\")\n",
    "print(f\"'y': {round(odom1['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom1['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto2):\n",
    "    time.sleep(0.05)\n",
    "odom2 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto2:\")\n",
    "print(f\"'x': {round(odom2['x'], 3)}\")\n",
    "print(f\"'y': {round(odom2['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom2['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto3):\n",
    "    time.sleep(0.05)\n",
    "odom3 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto3:\")\n",
    "print(f\"'x': {round(odom3['x'], 3)}\")\n",
    "print(f\"'y': {round(odom3['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom3['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto4):\n",
    "    time.sleep(0.05)\n",
    "odom4 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto4:\")\n",
    "print(f\"'x': {round(odom4['x'], 3)}\")\n",
    "print(f\"'y': {round(odom4['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom4['theta'], 3)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c325320b",
   "metadata": {},
   "source": [
    "We wait each time for the current goto to be finished before we read the odometry. As we see, the odometry is close to the target after the goto is declared as finished, as the default distance_tolerance is 5cm.  \n",
    "\n",
    "If we do now the same with the higher tolerances:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0e0debe0",
   "metadata": {},
   "outputs": [],
   "source": [
    "goto1 = reachy.mobile_base.goto(x=0.0, y=0.0, theta=0.0, distance_tolerance=0.1)\n",
    "goto2 = reachy.mobile_base.goto(x=0.6, y=0.0, theta=0.0, distance_tolerance=0.3)\n",
    "goto3 = reachy.mobile_base.goto(x=-0.2, y=0.0, theta=0.0, distance_tolerance=0.3)\n",
    "goto4 = reachy.mobile_base.goto(x=0.3, y=0.0, theta=0.0, distance_tolerance=0.1)\n",
    "\n",
    "while not reachy.is_goto_finished(goto1):\n",
    "    time.sleep(0.05)\n",
    "odom1 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto1:\")\n",
    "print(f\"'x': {round(odom1['x'], 3)}\")\n",
    "print(f\"'y': {round(odom1['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom1['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto2):\n",
    "    time.sleep(0.05)\n",
    "odom2 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto2:\")\n",
    "print(f\"'x': {round(odom2['x'], 3)}\")\n",
    "print(f\"'y': {round(odom2['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom2['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto3):\n",
    "    time.sleep(0.05)\n",
    "odom3 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto3:\")\n",
    "print(f\"'x': {round(odom3['x'], 3)}\")\n",
    "print(f\"'y': {round(odom3['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom3['theta'], 3)}\")\n",
    "\n",
    "while not reachy.is_goto_finished(goto4):\n",
    "    time.sleep(0.05)\n",
    "odom4 = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto4:\")\n",
    "print(f\"'x': {round(odom4['x'], 3)}\")\n",
    "print(f\"'y': {round(odom4['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom4['theta'], 3)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a6c67f16",
   "metadata": {},
   "source": [
    "We still wait for the gotos to be finished The next goto starts as soon as the tolerance of the current one is reached, so the mobile base is further to the target when switching to the next command.  \n",
    "\n",
    "When there is no more queued goto, the mobile base tries to reach the target position, even if the *goto* was declared finished. If we ask again for the odometry, without asking any new goto, the robo has finally reached its target:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e21ef602",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(1)\n",
    "odom4_bis = reachy.mobile_base.odometry\n",
    "print(\"Odometry after goto4:\")\n",
    "print(f\"'x': {round(odom4_bis['x'], 3)}\")\n",
    "print(f\"'y': {round(odom4_bis['y'], 3)}\")\n",
    "print(f\"'theta': {round(odom4_bis['theta'], 3)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61557400",
   "metadata": {},
   "source": [
    "> Note : setting the tolerances to 0 will make the target impossible to reach in most cases. You will have to wait for the goto timeout to be over before the mobile base will start another goto."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cc91417a",
   "metadata": {},
   "source": [
    "### Goto timeout"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17cd4e9f",
   "metadata": {},
   "source": [
    "The timeout will stop the movement whatever the current position is if the target has not been reached before the timeout time is over.  \n",
    "In the case the target has been reached, the timeout has no effect.  \n",
    "\n",
    "Contrary to the tolerances, a **goto finished by a timeout does also end the real movement on the robot**, which no more commands will be sent after the timeout and the robot will stop.  \n",
    "\n",
    "\n",
    "To see how it works, let's send the mobile base back to its base position and then send a goto with a very low timeout:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9572527f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Go back to position\n",
    "reachy.mobile_base.goto(x=0.0, y=0.0, theta=0.0, wait=True)\n",
    "\n",
    "# Use a low timeout\n",
    "reachy.mobile_base.goto(x=0.4, y=0.0, theta=90.0, timeout=0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7b02a39b",
   "metadata": {},
   "source": [
    "\n",
    "If we check the odometry now:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1cfeb188",
   "metadata": {},
   "outputs": [],
   "source": [
    "time.sleep(0.5)\n",
    "print(\"Odometry:\")\n",
    "print(f\"'x': {round(reachy.mobile_base.odometry['x'], 5)}\")\n",
    "print(f\"'y': {round(reachy.mobile_base.odometry['y'], 2)}\")\n",
    "print(f\"'theta': {round(reachy.mobile_base.odometry['theta'], 2)}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1b59acce",
   "metadata": {},
   "source": [
    "The distance to the tolerance is still not good, but the movement has stopped after the given timeout of 5 seconds."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e8243ee0",
   "metadata": {},
   "source": [
    "> Default timeout is **100 seconds**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "68eed018",
   "metadata": {},
   "source": [
    "You can also use the timeout to interrupt a goto after a certain amount of time in a given direction. For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2bfb00e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Go back to base position\n",
    "reachy.mobile_base.goto(x=0, y=0.0, theta=0.0, wait=True)\n",
    "\n",
    "# Start movement torwards x=1m\n",
    "timeout_goto = reachy.mobile_base.goto(x=1, y=0.0, theta=0.0, timeout=2)\n",
    "tic = time.time()\n",
    "while not reachy.is_goto_finished(timeout_goto):\n",
    "    time.sleep(0.05)\n",
    "print(f\"goto interrupted after {round(time.time() - tic, 2)} seconds\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "097efbb0",
   "metadata": {},
   "source": [
    "Be careful that the mobile does not precisely follow the expected trajectory to reach the target position."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6c4e1bfe",
   "metadata": {},
   "source": [
    "## Advanced usage"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "42c405f4",
   "metadata": {},
   "source": [
    "### Set speed"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14",
   "metadata": {},
   "source": [
    "The speed of the movement can be defined using the command `set_goal_speed()`. It requires to set all 3 target speeds *vx*, *vy* and *vtheta*.\n",
    "Send then the command to the robot with `send_speed_command()`.\n",
    "\n",
    "> **This will assign speed to the robot for 200ms**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.set_goal_speed(vx=0.5, vy=0.0, vtheta=0)\n",
    "tic=time.time()\n",
    "while time.time()-tic < 2:\n",
    "    reachy.mobile_base.send_speed_command()\n",
    "    time.sleep(0.01)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "205b82e0",
   "metadata": {},
   "source": [
    "Setting speed can be useful when replaying movements for example."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "16",
   "metadata": {},
   "source": [
    "## Free wheel"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "958db21d",
   "metadata": {},
   "source": [
    "In order to be able to move easily the mobile base manually (pushing the robot for example), you can turn_off the mobile like other parts:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "17",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.mobile_base.turn_off()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
